#include "draw.h"


using namespace markerAR;


Mat drawTrackLine(const Mat &img,const vector<KeyPoint> &origin,const vector<KeyPoint> &now,const vector<DMatch> &dmatch,Scalar color,int thickness)
{
	Mat ret = img.clone();
	for (int j = 0; j < dmatch.size();++j)
	{
		auto &i = dmatch[j];
		line(ret, origin[i.trainIdx].pt, now[i.queryIdx].pt, color, thickness);
	}
	return ret;
}

Mat drawTrackLine(const Mat&img, const vector<Point2f> &origin, const vector<Point2f> &now, Scalar color, int thickness)
{
	CV_Assert(origin.size() == now.size());
	Mat ret = img.clone();
	for (int i = 0; i < origin.size(); ++i)
	{
		line(ret, origin[i], now[i], color, thickness);
	}
	return ret;
}


Mat drawInfo(const Mat &img, const string &str, const Point2i &pos, Scalar color, int thickness)
{
	Mat ret = img.clone();
	putText(ret, str, pos, FONT_HERSHEY_PLAIN, 1, color, thickness);
	return ret;
}


void drawPoints(const Mat&img, const vector<Point2f> &pts2d,Mat &out)
{
	out = img.clone();
	for (size_t i = 0; i < pts2d.size(); i++)
	{
		circle(out, pts2d[i], 3, CV_RGB(255, 128, 64), 2);
	}
}


void drawMatches(const Mat&left,const Mat&right,const vector<Eigen::Vector2f>&leftPoints,const vector<Eigen::Vector2f>&rightPoints,Mat&output)
{
  CV_Assert(left.size() == right.size());
  CV_Assert(left.type() == right.type());
  output = Mat(left.rows,left.cols+right.cols,left.type());
  for (int i=0; i<output.rows; ++i) {
    auto p1 = output.ptr<uchar>(i);
    auto p2 = p1+left.cols;
    memcpy(p1, left.ptr<uchar>(i), sizeof(uchar)*left.cols);
    memcpy(p2, right.ptr<uchar>(i), sizeof(uchar)*right.cols);
  }
  for (int i=0; i<leftPoints.size(); ++i) {
    line(output, Point(leftPoints[i].x(),leftPoints[i].y()), Point(rightPoints[i].x()+left.cols,rightPoints[i].y()), Scalar(1,0,0,1));
  }
}


void drawPoints(const Mat&img, const vector<Feature*> &pts2d, Mat &out)
{
    
    out = img.clone();
    Point2f a;
    for (size_t i = 0; i < pts2d.size(); i++)
    {
        Converter::Eigen2CV(pts2d[i]->_pos, a);
        circle(out, a, 3, CV_RGB(255, 128, 64), 2);
    }
}